#include "FragmentationDemo.h"
#include "Screen.h"
#include "Timer.h"
#include "EventPoller.h"
#include "TestCuboid.h"
#include <assert.h>
#include <btBulletDynamicsCommon.h>

FragmentationDemo::FragmentationDemo() : m_pFractureCuboid(0), m_pGround(0)
{
	//Set up user input
	TheEventPoller::Instance()->AddHandler(this);

	//Glut init
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	gluPerspective(45,TheScreen::Instance()->GetScreenRatio(),1,1000);
	gluLookAt(25, 25, 55.0, 0, 7, 0, 0, 1.0, 0);

	//Lighting init
	m_light.SetPositionalLight(Vec3f(20, 5, 25));
	m_light.SetAmbient(1, 1, 1, 1.0f);
	m_light.SetSpecular( 1, 1.0f,1.0f, 1.0f);
	m_light.SetDiffuse(1, 1.0f, 1.0f, 1.0f);
	m_light.LightOn();

	//Init physics
	InitPhysics();

	//Create cuboid to fracture
	m_pFractureCuboid = new TestCuboid(Vec3f(0, 25, -5), Vec3f( 2,8,1), 12);

	m_pGround = new Ground(Vec3f(0, 0, 0), 20);

}

void FragmentationDemo::ResetScene()
{
	delete m_pFractureCuboid;
	delete m_pGround;

	ExitPhysics();
	InitPhysics();

	m_pFractureCuboid = new TestCuboid(Vec3f(0, 25, -5), Vec3f( 2,8,1), 15);
	m_pGround = new Ground(Vec3f(0, 0, 0), 20);

}

bool FragmentationDemo::InitPhysics()
{
	//Bullet initialisation
	m_pBroadphase = new btDbvtBroadphase;
	m_pCollisionConfiguration = new btDefaultCollisionConfiguration();
	m_pDispatcher = new	btCollisionDispatcher(m_pCollisionConfiguration);
	m_pSolver = new btSequentialImpulseConstraintSolver;

	m_pDynamicsWorld = new btDiscreteDynamicsWorld(m_pDispatcher,m_pBroadphase,m_pSolver,m_pCollisionConfiguration);
	m_pDynamicsWorld->setGravity(btVector3(0,-10,0));

	

	return true;
}

void FragmentationDemo::ExitPhysics()
{
	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_pDynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_pDynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_pDynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear();

	delete m_pDynamicsWorld;
	
	delete m_pSolver;
	
	delete m_pBroadphase;
	
	delete m_pDispatcher;

	delete m_pCollisionConfiguration;
}

void FragmentationDemo::Update()
{
	TheTimer::Instance()->Update();
	TheEventPoller::Instance()->Update();

	m_pDynamicsWorld->stepSimulation(TheTimer::Instance()->GetDT());
	BTCheckPairs();
	SafelyAddRemoveRigidBodies();

}

void FragmentationDemo::Draw()
{

	m_pGround->Draw();
	m_pFractureCuboid->Draw();
}

btDynamicsWorld* FragmentationDemo::GetDynamicsWorld()
{
	return m_pDynamicsWorld;
}
void FragmentationDemo::AddCollisionShape(btCollisionShape* shape)
{
	m_collisionShapes.push_back(shape);
}

void FragmentationDemo::SafelyAddRemoveRigidBodies()
{
	AddRemoveBodies::iterator it = m_addRemoveBodies.begin();
	AddRemoveBodies::iterator end = m_addRemoveBodies.end();

	for( ; it != end; ++it)
	{
		if(it->first)
		{
			m_pDynamicsWorld->addRigidBody(it->second);
		}
		else
		{
			m_pDynamicsWorld->removeRigidBody(it->second);
		}
	}
	m_addRemoveBodies.clear();
}

void FragmentationDemo::BTCheckPairs()
{
	//Iterate through contact pairs to dispatch custom collision callbacks
	static bool firstColl = false;
	if(!firstColl)
	{

		int i, j, numContacts;
	
		int numManifolds = m_pDynamicsWorld->getDispatcher()->getNumManifolds();

		btPersistentManifold* manifold;

		for ( i = 0; i < numManifolds; i++)
		{
			manifold =  m_pDynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);

			btCollisionObject* colObj0 = const_cast<btCollisionObject*>(manifold->getBody0());
			btCollisionObject* colObj1 = const_cast<btCollisionObject*>(manifold->getBody1());


			btRigidBody* body0 = btRigidBody::upcast(colObj0);
			btRigidBody* body1 = btRigidBody::upcast(colObj1);


			numContacts = manifold->getNumContacts();
			for ( j=0; j < numContacts; j++)
			{
				btManifoldPoint& pt = manifold->getContactPoint(j);
				if (pt.getDistance()<0.f)
				{
					PhysicsBody* pb0 = reinterpret_cast<PhysicsBody*>(body0->getUserPointer());
					PhysicsBody* pb1 = reinterpret_cast<PhysicsBody*>(body1->getUserPointer());


					pb0->Collided(pt);
				
					pb1->Collided(pt);
				

				}
			}
		}
	}
}

void FragmentationDemo::AddOrRemoveRigidBody(bool addOrRemove, btRigidBody* body)
{
	m_addRemoveBodies.push_back(std::make_pair(addOrRemove, body));
}

void FragmentationDemo::OnKeyboardEvent(const SDL_KeyboardEvent& k)
{
	//Press 'R' To reset the simulation
	if(k.keysym.sym == SDLK_r && k.state == SDL_PRESSED)
	{
		ResetScene();
	}
}